#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <vector>
#include <opencv2/opencv.hpp>
#include "dragon.h"
#include "mynode_cv.h"

void ResconstructTree(cv::KDTree& tree, int fatherid, std::vector<myNodeCv>& node_details)
{

    if(fatherid<0){
        //如果遇到了叶子
        node_details[father_id].Es = E.row(std::abs(tree.Nodes[father_id].idx) -1).cast<float>();
        node_details[father_id].pointmin[0] = node_details[father_id].pointmax[0] = node_details[father_id].Pv[0] =
                vertices[std::abs(tree.Nodes[father_id].idx)-1][0];
        node_details[father_id].pointmin[1] = node_details[father_id].pointmax[1] = node_details[father_id].Pv[1] =
                vertices[std::abs(tree.Nodes[father_id].idx)-1][1];
        node_details[father_id].pointmin[2] = node_details[father_id].pointmax[2] = node_details[father_id].Pv[2] =
                vertices[std::abs(tree.Nodes[father_id].idx)-1][2];
    }
    ResconstructTree(tree, tree.nodes[fatherid].left, node_details);
    ResconstructTree(tree, tree.nodes[fatherid].right, node_details);
    int leftchild = tree.nodes[father_id].left,
            rightchild = tree.nodes[father_id].right;
    node_details[father_id].Es = tree.Nodes[leftchild].Es + tree.Nodes[rightchild].Es;
    for(int i=0;i<3;i++){
        node_details[father_id]. = tree.Nodes[leftchild -1].AreaMax.coeffRef(0,i)>=tree.Nodes[rightchild - 1].AreaMax.coeffRef(0,i)?
                    tree.Nodes[leftchild -1].AreaMax.coeffRef(0,i):tree.Nodes[rightchild - 1].AreaMax.coeffRef(0,i);
        tree.Nodes[father_id-1].AreaMin.coeffRef(0,i) = tree.Nodes[leftchild -1].AreaMin.coeffRef(0,i)<=tree.Nodes[rightchild - 1].AreaMin.coeffRef(0,i)?
                    tree.Nodes[leftchild -1].AreaMin.coeffRef(0,i):tree.Nodes[rightchild - 1].AreaMin.coeffRef(0,i);
    }
    if(tree.Nodes[leftchild -1].idx<0&&tree.Nodes[rightchild - 1].idx<0){
        //左右都是叶子；
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,0);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,0);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }
    }
    else if(tree.Nodes[leftchild -1].idx>=0&&tree.Nodes[rightchild - 1].idx<0){
        //左边不是叶子，右边是叶子
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,i);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,0);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }


    }
    else if(tree.Nodes[leftchild -1].idx<0&&tree.Nodes[rightchild - 1].idx<0){
        //左是叶子, 右不是叶子；
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,0);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,i);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }
    }

    else if(tree.Nodes[leftchild -1].idx>=0&&tree.Nodes[rightchild - 1].idx>=0){
        //左边不是叶子，右边也不是叶子
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,i);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,i);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }
    }
    return;
}

int main()
{
    //读树
    std::ifstream in("treecv.txt",std::ios::binary);
    int tree_node_size;
    in.read((char*)&tree_node_size, sizeof(int));
    cv::KDTree tree;
    for(int i=0;i<tree_node_size;i++){
        cv::KDTree::Node curnode;
        in.read((char*)&(curnode.idx), sizeof(int));
        in.read((char*)&(curnode.left), sizeof(int));
        in.read((char*)&(curnode.right), sizeof(int));
        in.read((char*)&(curnode.boundary), sizeof(float));
        tree.nodes.push_back(curnode);
    }
    std::vector<myNodeCv> node_details(tree_node_size);
    ResconstructTree(tree, 0);
}
